#include "move.hpp"
UINT8 msgunlock[41] = {
0xfe, 0x21, 0xae, 0xff, 0xbe, 0x4c, 0, 0, 0x80, 0x3f, 0,
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
0, 0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0xcd, 0xbb};
UINT8 msglock[41] = {
0xfe, 0x21, 0xb6, 0xff, 0xbe, 0x4c, 0, 0, 0x00, 0x00, 0, 
0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 
0, 0, 0, 0, 0x90, 0x01, 0x01, 0x01, 0, 0x94, 0x5d};

void base_controller::synchronize(unsigned char buffer)
{
	mavlink_message_t msg;
	mavlink_status_t status;
	if (mavlink_parse_char(0, buffer, &msg, &status))
	{
		if (msg.msgid == 0)
		{
			mavlink_heartbeat_t ht;
			mavlink_msg_heartbeat_decode(&msg, &ht);
			droneStatus.custom_mode = ht.custom_mode;
			droneStatus.base_mode = ht.base_mode;
			droneStatus.sys_status = ht.system_status;
		}
		if (msg.msgid == 253)
		{
			mavlink_statustext_t tp;
			mavlink_msg_statustext_decode(&msg, &tp);
			tp.text[49] = 0;
			mav_errmsg = string(tp.text);
		}
		else if (msg.msgid == 30)
		{
			mavlink_attitude_t attitude;
			mavlink_msg_attitude_decode(&msg, &attitude);
			pose.pitchmsg = attitude.pitch;
			pose.rollmsg = attitude.roll;
			pose.yawmsg = attitude.yaw;
		}
		else if (msg.msgid == 74)
		{
			mavlink_vfr_hud_t vfr_hud;
			mavlink_msg_vfr_hud_decode(&msg, &vfr_hud);
			pose.deepmsg = vfr_hud.alt+deepBias;
		}
		else if (msg.msgid == 1)
		{
			mavlink_sys_status_t sys_ss;
			mavlink_msg_sys_status_decode(&msg, &sys_ss);
			droneStatus.battery_v = sys_ss.voltage_battery;
		}
	}
}

int xbias,ybias,zbias,rbias;
void base_controller::setTargetVel(Veloc v)
{
	this->vel.x = v.x+xbias;
	this->vel.y = v.y+ybias;
	this->vel.z = v.z+zbias;
	this->vel.r = v.r+rbias;
}

inline void base_controller::doCtrl()
{
	mavlink_msg_manual_control_send(chan,target,vel.x,vel.y,vel.z,vel.r,0);
}

void base_controller::changeMode(int newMode)
{
	assert(newMode>=0);
	assert(newMode<21);
	mavlink_msg_set_mode_send(chan, target, 1, newMode);
}

void base_controller::arm_disarm(bool arm)
{
	if(arm)
		this->serial.async_write_some(buffer(msgunlock, 41), std::bind([]() {}));
	else
		this->serial.async_write_some(buffer(msglock, 41), std::bind([]() {}));
}

void base_controller::pixHeartBeat()
{
	mavlink_msg_heartbeat_send(chan, 1, MAV_AUTOPILOT_GENERIC, 0, 0, 0);
}


//for async usage
void base_controller::async_doCtrl(steady_timer* timer){
	doCtrl();
	timer->expires_from_now(asio::chrono::milliseconds(500));
	timer->async_wait(std::bind(&base_controller::async_doCtrl,this,timer));
}
void base_controller::async_pixHeartBeat(steady_timer* timer){
	pixHeartBeat();
	timer->expires_from_now(asio::chrono::milliseconds(700));
	timer->async_wait(std::bind(&base_controller::async_doCtrl,this,timer));
}